#include <bits/stdc++.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <signal.h>

#define GREEN_LIGHT 1
#define RED_LIGHT   0

using namespace std;
using namespace cv;
Mat img;

double calPerimeter(vector<vector<Point>> contours)
{
    double max_perimeter;
    for ( const auto& contour : contours ) {
        max_perimeter = arcLength(contour, true) > max_perimeter ? arcLength(contour, true) : max_perimeter;
    }
    return max_perimeter;
}

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    try {
        // 提取红色和绿色轮廓
        Mat                   mask_red, mask_green, kernel, img_erode;
        vector<vector<Point>> contours_red, contours_green;
        double                perimeter_red, perimeter_green;
        img    = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
        kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
        erode(img, img_erode, kernel, Point(-1, -1), 2);
        inRange(img_erode, Scalar(16, 16, 136), Scalar(67, 67, 255), mask_red);
        inRange(img_erode, Scalar(0, 60, 0), Scalar(60, 255, 50), mask_green);
        findContours(mask_red, contours_red, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
        findContours(mask_green, contours_green, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

        // 计算轮廓长度
        perimeter_red   = calPerimeter(contours_red);
        perimeter_green = calPerimeter(contours_green);
        if ( perimeter_red > 200 ) {
            ros::param::set("light", RED_LIGHT);
        }
        else if ( perimeter_green > 300 ) {
            ros::param::set("light", GREEN_LIGHT);
        }
    }
    catch ( cv_bridge::Exception& e ) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "light");
    ros::NodeHandle nh;
    ros::Subscriber img_rcv = nh.subscribe("/usb_cam/image_raw", 3, &imageCallback);

    ros::Rate looprate(50);
    while ( ros::ok() ) {
        ros::spinOnce();
        looprate.sleep();
    }
}
